#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <bits/stdc++.h>
#include "fyt_msg/SerialSendMsg.h"

static std::queue<ros::Time> time_buf;

//从 img_catcher 包开始计时
void callBackBegin(const sensor_msgs::ImageConstPtr &msg){
    ros::Time t;
    t.sec=ros::Time::now().sec;
    t.nsec=ros::Time::now().nsec;
    time_buf.push(t);
}

//在 solve 部分结束后结束计时
void callBackEnd(const fyt_msg::SerialSendMsgConstPtr &msg){
    ros::Time t;
    ros::Time t0=time_buf.front();
    time_buf.pop();
    t.sec=ros::Time::now().sec;
    t.nsec=ros::Time::now().nsec;

    int dt;
    if(t0.sec<t.sec) dt=1e9;
    else dt=0;

    dt+=(t.nsec-t0.nsec);
    ROS_INFO("exe time: %d",dt);
}

int main(int argc,char**argv){
    ros::init(argc,argv,"calc_time_node");
    ros::NodeHandle n;
    
    ros::Subscriber sub_begin=n.subscribe("raw_img",1,callBackBegin);
    ros::Subscriber sub_end=n.subscribe("serial_send",1,callBackEnd);
    ros::spin();

    return 0;
}